/*
 *  MotorController_test.cpp
 *
 *  Created by: Kellen Carey
 *  Date: 11/14/2013
 */

#include "MotorController.h"
#include <iostream>
#include <string>
#include <time.h>
#include <sstream>

using namespace std;

MotorController motorController;

string input;

void motionTesting();
void createNewMotion();

int main(int argc, char* argv[]) {

	motorController.setMotion(STAND);
	motorController.step(false);


	bool done = false;

	do {
		cout << "W = Walk\nQ = Left turn\nE = Right turn\nS = Stand\nA = Side step left\nD = Side step right\nK = Kick\nN = Edited animation\nX = exit " << std::endl;
		std::cout << "T for editing motions and individual testing";

		cin >> input;

		if (input == "W" || input == "w") {
			motorController.setMotion(WALK);
			cout << "Walking" << endl;
		}
		else if (input == "Q" || input == "q") {
			motorController.setMotion(TURNLEFT);
			cout << "Turning Left" << endl;
		}
		else if (input == "E" || input == "e") {
			motorController.setMotion(TURNRIGHT);
			cout << "Turning Right" << endl;
		}
		else if (input == "S" || input == "s") {
			motorController.setMotion(STAND);
			cout << "Standing" << endl;
		}
		else if (input == "A" || input == "a") {
			motorController.setMotion(SIDESTEPLEFT);
			cout << "Stepping Left" << endl;
		}
		else if (input == "D" || input == "d") {
			motorController.setMotion(SIDESTEPRIGHT);
			cout << "Stepping Right" << endl;
		}
		else if (input == "K" || input == "k") {
			motorController.setMotion(KICK);
			cout << "Kicking (left foot)" << endl;
		}
		else if (input == "N" || input == "n")
		{
			motorController.setMotion(ANIMATION);
			cout << "Executing Edited Animation" << endl;
		}
		else if (input == "X" || input == "x"){
			done = true;
		}
		else if (input == "T" || input == "t") {
			motionTesting();
		}
		else {
			cout << "Please enter a valid input" << endl;
		}
		clock_t startTime = clock();

		while ((((float)clock() - (float)startTime)/CLOCKS_PER_SEC) < 7 && !done) {
			motorController.step(false);
		}

	} while (!done);

	return 0;
}

void motionTesting() {
	bool FinishMotionTest= false;
	bool motionTestingDone = false;
	while(!FinishMotionTest) {

		motorController.lockHead();
		motionTestingDone = false;

		std::cout << "We are now testing individual motions, with the ability to transfer between and edit steps" << std::endl;
		std::cout << "Which motion would you like to test?" << std::endl;
		cout << "W = Walk\nQ = Left turn\nE = Right turn\nS = Stand\nA = Side step left\nD = Side step right\nK = Kick\nN = Edited animation\nX = exit " << std::endl;


		cin >> input;

		if (input == "W" || input == "w")
		{
			motorController.setMotion(WALK);
		}
		else if (input == "Q" || input == "q")
		{
			motorController.setMotion(TURNLEFT);
		}
		else if (input == "E" || input == "e")
		{
			motorController.setMotion(TURNRIGHT);
		}
		else if (input == "S" || input == "s")
		{
			motorController.setMotion(STAND);
		}
		else if (input == "A" || input == "a")
		{
			motorController.setMotion(SIDESTEPLEFT);
		}
		else if (input == "D" || input == "d")
		{
			motorController.setMotion(SIDESTEPRIGHT);
		}
		else if (input == "K" || input == "k")
		{
			motorController.setMotion(KICK);
		}
		else if (input == "N" || input == "n")
		{
			motorController.setMotion(ANIMATION);
		}
		else if (input == "X" || input == "x"){
			return;
		}
		else if (input == "L" || input == "L") {
			createNewMotion();
			motionTestingDone = true;
		}

		else{
			cout << "Please enter a valid input" << endl;
			motionTestingDone = true;
		}


		while (!motionTestingDone) {
			std::cout << "n ------> Next step "<<std::endl;
			//std::cout << "p -> Previous step  "<<std::endl;
			std::cout << "r ------> Release *SINGLE* Motor  "<<std::endl;
			std::cout << "ra -----> Release *ALL* Motors  "<<std::endl;
			std::cout << "e ------> Engage *SINGLE* Motor  "<<std::endl;
			std::cout << "ea -----> Engage *ALL* Motors  "<<std::endl;
			std::cout << "f ------> Finish the current motion " << std::endl;
			std::cout << "rl -----> Release Left Leg " << std::endl;
			std::cout << "rr -----> Release Right Leg " << std::endl;
			std::cout << "er -----> Enable Right Leg " << std::endl;
			std::cout << "el -----> Enable Left Leg " << std::endl;
			std::cout << "rp -----> Read Position " << std::endl;
			std::cout << "shift+s -----> Save Positions step " << std::endl;
			std::cout << "shift+p -----> Print current Motion " << std::endl;
			cin >> input;


			if (input == "n" || input == "N") {

				/**
				 *  If the incrementStep function returns false, then the while loop continues, and 			waits for user input.
				 *  If it returns true, that means the motion has no more steps and the while loop 			exits
				 */
				motorController.incrementIndex();
				motionTestingDone= motorController.incrementStep();
				motorController.decrementIndex();
				if(motionTestingDone==true) {
					bool validInput = false;
					while (!validInput) {
						std::cout<<"Motion is over. Test another motion?  (Y/N)"<<std::endl;
						cin >> input;
						if(input == "Y" || input == "y") {
							FinishMotionTest= false;
							validInput = true;
						}
						else if(input == "N" || input == "n") {
							FinishMotionTest= true;
							validInput = true;
						}
						else {
							std::cout << "Please enter a valid input" << std::endl;
						}
					}


				}
				//motorController.executeNext(motorController.getMotionFile());
			}

//			else if (input == "p" || input == "P") {
//
//				motorController.incrementIndex();
//				motorController.executePrevious(motorController.getMotionFile());
//			}
			else if (input == "r" || input == "R") {
				//release single motor
				std::cout <<"Which motor would you like to release? ";
				std::string tempinput;
				cin >> tempinput;
				int motor = 0;
				std::stringstream(tempinput) >> motor;
				motorController.disableMotor(motor);
			}
			else if (input == "ra" || input == "RA") {
				//release all motors
				motorController.disableAllMotors();
			}
			else if (input == "e" || input == "E") {
				//engage single motor
				std::cout <<"Which motor would you like to engage? ";
				std::string tempinput;
				cin >> tempinput;
				int motor;
				std::stringstream(tempinput) >> motor;
				motorController.enableMotor(motor);
			}
			else if (input == "ea" || input == "EA") {
				//engage all motors
				motorController.enableAllMotors();

			}
			else if (input == "f" || input == "F") {
				clock_t startTime = clock();
				while ((((float)clock() - (float)startTime)/CLOCKS_PER_SEC) < 5) {
					motorController.step(false);
				}
				motionTestingDone = true;
			}
			else if (input == "er" || input == "ER") {
				motorController.enableRightLeg();
			}
			else if (input == "el" || input == "EL") {
				motorController.enableLeftLeg();
			}
			else if (input == "dr" || input == "DR") {
				motorController.disableRightLeg();
			}
			else if (input == "dl" || input == "DL") {
				motorController.disableLeftLeg();
			}
			else if (input == "rp" || input == "RP") {
				std::string tempinput;
				int motor;
				std::cout << "Which motor would you like to read? ";
				std::cin >> tempinput;
				std::stringstream(tempinput)>>motor;
				std::cout << motorController.readMotorPosition(motor) << std::endl;
			}
			else if (input == "S") {
				std::string getTime;
				int time;
				std::cout << "How long should this step take?  ";
				std::cin >> getTime;
				std::stringstream(getTime)>>time;
				motorController.setTime(time);

			}
			else if (input == "P") {
				std::string printOption;
				int printDecision;
				std::cout << "Print to file or console? (f/c) "<<std::endl;
				std::cin >> printOption;

				if(printOption=="c" ||printOption=="C"){
					printDecision= 1;
				}
				if(printOption=="f" ||printOption=="F"){
					printDecision= 2;
				}


				motorController.printMotion(printDecision);
			}

			else {
				std::cout << "Please enter a valid input" << std::endl;
			}
		}
	}
}

void createNewMotion() {
	motorController.setMotion(TEMP_ANIMATION);
	motorController.addMotion();
	while (true) {
		std::cout << "n ------> Create a new step "<<std::endl;
		std::cout << "r ------> Release *SINGLE* Motor  "<<std::endl;
		std::cout << "ra -----> Release *ALL* Motors  "<<std::endl;
		std::cout << "e ------> Engage *SINGLE* Motor  "<<std::endl;
		std::cout << "ea -----> Engage *ALL* Motors  "<<std::endl;
		std::cout << "rl -----> Release Left Leg " << std::endl;
		std::cout << "rr -----> Release Right Leg " << std::endl;
		std::cout << "er -----> Enable Right Leg " << std::endl;
		std::cout << "el -----> Enable Left Leg " << std::endl;
		std::cout << "rp -----> Read Position " << std::endl;
		std::cout << "shift+s -----> Save Positions step " << std::endl;
		std::cout << "shift+p -----> Print current Motion " << std::endl;
		std::cout << "d -----> Done editing motion" << std::endl;
		cin >> input;


		if (input == "n" || input == "N") {
			// Create a new step
			motorController.addMotionStep();
		}

		else if (input == "r" || input == "R") {
			//release single motor
			std::cout <<"Which motor would you like to release? ";
			std::string tempinput;
			cin >> tempinput;
			int motor = 0;
			std::stringstream(tempinput) >> motor;
			motorController.disableMotor(motor);
		}
		else if (input == "ra" || input == "RA") {
			//release all motors
			motorController.disableAllMotors();
		}
		else if (input == "e" || input == "E") {
			//engage single motor
			std::cout <<"Which motor would you like to engage? ";
			std::string tempinput;
			cin >> tempinput;
			int motor;
			std::stringstream(tempinput) >> motor;
			motorController.enableMotor(motor);
		}
		else if (input == "ea" || input == "EA") {
			//engage all motors
			motorController.enableAllMotors();

		}
		else if (input == "er" || input == "ER") {
			motorController.enableRightLeg();
		}
		else if (input == "el" || input == "EL") {
			motorController.enableLeftLeg();
		}
		else if (input == "dr" || input == "DR") {
			motorController.disableRightLeg();
		}
		else if (input == "dl" || input == "DL") {
			motorController.disableLeftLeg();
		}
		else if (input == "rp" || input == "RP") {
			std::string tempinput;
			int motor;
			std::cout << "Which motor would you like to read? ";
			std::cin >> tempinput;
			std::stringstream(tempinput)>>motor;
			std::cout << motorController.readMotorPosition(motor) << std::endl;
		}
		else if (input == "S") {
			std::string getTime;
			int time;
			std::cout << "How long should this step take?  ";
			std::cin >> getTime;
			std::stringstream(getTime)>>time;
			motorController.setTime(time);

		}
		else if (input == "P") {
			std::string printOption;
			int printDecision;
			std::cout << "Print to file or console? (f/c) "<<std::endl;
			std::cin >> printOption;

			if(printOption=="c" ||printOption=="C"){
				printDecision= 1;
			}
			if(printOption=="f" ||printOption=="F"){
				printDecision= 2;
			}


			motorController.printMotion(printDecision);
		}
		else if (input == "d" || input == "D") {
			//motorController.printMotion(2);
			return;
		}

		else {
			std::cout << "Please enter a valid input" << std::endl;
		}
	}
}
